#ifndef _SIUT_SIMD_STANDARD_RAY_HPP_
#define _SIUT_SIMD_STANDARD_RAY_HPP_

#include <limits>

#include "Vec3f.hpp"
#include "BBox3f.hpp"
#include "BSphere3f.hpp"
#include "HPlane3f.hpp"
namespace siut
{
namespace simd
{
namespace standard
{
  struct Ray3f
  {
    Vec3f pos_;
    Vec3f dir_;
  };

  inline static Ray3f fromEndpoints(const Vec3f &v, const Vec3f &vec)
  {
    Ray3f ret;
    ret.pos_ = v;
    ret.dir_ = vec - v;
    return ret;
  }

  inline const Vec3f& getOrigin(const Ray3f &r) 
  {
    return r.pos_;
  }

  inline const Vec3f& getDirection(const Ray3f &r) 
  {
    return r.dir_;
  }

  inline Vec3f timeStep(Ray3f &r, float t) 
  {
    return r.pos_ + t * r.dir_;
  }

  inline bool intersectRayBBox(float &t, const Ray3f &r, const BBox3f &b)
  {
    Vec3f p = getCenter(b) - getOrigin(r);
    Vec3f d = getDirection(r);
    Vec3f l = getSizes(b);
    Vec3f h = 0.5 * l;

    float fmin = - std::numeric_limits<float>::infinity();
    float fmax =  std::numeric_limits<float>::infinity();
    float e, f;
    for(size_t i = 0; i < 3; i++)
      {
	e = p[i];
	f = d[i];
	if(fabs(f) > std::numeric_limits<float>::epsilon())
	  {
	    float t1 = (e + h[i]) / f;
	    float t2 = (e - h[i]) / f;
	    
	    if(t1 > t2) 
	      {
		std::swap(t1, t2);
	      }
	    if(t1 > fmin)
	      {
		fmin = t1;
	      }
	    if(t2 < fmax)
	      {
		fmax = t2;
	      }
	    if(fmin > fmax || fmax < 0)
	      {
		return false;
	      }
	  }
	else if(-e - h[i] > 0)
	  {
	    return false;
	  }
	else if(-e + h[i] < 0)
	  {
	    return false;
	  }
      }
    if(fmin > 0)
      {
	t = fmin;
	return true;
      }
    else
      {
	t = fmax;
	return true;
      }
  }

  inline bool intersectRayHPlane(float &t, const Ray3f &r, const HPlane3f &hpl)
  {
    Vec3f o = getOrigin(r);
    Vec3f d = getDirection(r);
    Vec3f n = getNormal(hpl);
    
    float ndotd = dot(n, d);
    if(fabs(ndotd) > std::numeric_limits<float>::epsilon())
      {
	t = - (dot(n, o) + getOffset(hpl)) / ndotd;
	return true;
      }

    return false;
  }

  inline bool intersectRayTriangle(float &t, float &u, float &v, const Ray3f &ray,
			    const Vec3f &v0, const Vec3f &v1, const Vec3f &v2)
  {
    const Vec3f e1 = v1 - v0;
    const Vec3f e2 = v2 - v0;
    const Vec3f d = getDirection(ray);
    const Vec3f p = cross(d, e2);

    float dotten = dot(e1, p);
    if(fabs(dotten) < std::numeric_limits<float>::epsilon())
      {
	return false;
      }


    const Vec3f s = getOrigin(ray) - v0;
    dotten = 1.f / dotten;
    float _u = dotten * dot(s, p);
    if(_u < 0.f || _u > 1.f)
      {
	return false;
      }


    const Vec3f q = cross(s, e1);
    float _v = dotten * dot(d, q);
    if(_v < 0.f || _v > 1.f)
      {
	return false;
      }

    t = dotten * dot(e2, q);
    u = _u;
    v = _v;

    return true;
  }

  inline bool intersectRaySphere(float &t, const Vec3f &ray_o, const Vec3f &ray_d,
			  const Vec3f &sph_o, float sph_r)
  {
    const Vec3f l = sph_o - ray_o;
    float s = dot(l, ray_d);
    float ll = dot(l, l);
    float ss = sph_r * sph_r;
    if(s < 0.f && ll > ss)
      {
	return false;
      }

    float mm = ll - s * s;
    if(mm > ss)
      {
	return false;
      }

    float q = std::sqrt(ss - mm);
    t = ll > ss ? s - q : s + q;
    return true;
  }

  inline bool intersectRaySphere(float &t, const Ray3f &r, const BSphere3f &bs)
  {
    return intersectRaySphere(t, getOrigin((Ray3f&)r), getDirection(r),
			      getOrigin(bs), getRadius(bs));
  }


}//end namespaces
}
}

#endif
